#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "JacobisMethod.h"

//#define JM_DEBUG_IN
//#define JM_DEBUG
//#define JM_DEBUG_NVAL
//#define JM_DEBUG_OUT

#define JM_VERBOSE_OUT

double *JacobisMethod(struct extendedMatrix *M){
    unsigned long i, j, N, interactions=0;
    double E;
    double *Xk, *Xk_1;

    N = M->J_ORDER;
    Xk = (double*) calloc(N, sizeof(double));    
    Xk_1 = (double*) calloc(N, sizeof(double));    
    
    // Copies B into Xk (for the first interaction)
    for(i=0; i<N; i++){
        Xk[i] = M->vector[i];

        #ifdef JM_DEBUG_IN
            printf("B[%lu]=%lf\n", i, M->vector[i]);
        #endif
    }
    while(69){
        interactions++;
        // Copies Xk+1 into Xk (the next interaction)
        for(i=0; i<N; i++)
            Xk[i] = Xk_1[i];
    
        for(i=0; i<N; i++){
           Xk_1[i] = jmFindXi(M, i, Xk); 
        }

        // If E is positive, it passed the test
        E = numericValidation(M, Xk_1);

        #ifdef JM_DEBUG
            printf("Xk+1=%.1lf - Xk=%.1lf \
            ERROR=%.8lf MAX=%.4lf\n"
            , Xk_1[i], Xk[i], E , M->J_ERROR);
        #endif
       
        if(E>0){
            #ifdef JM_DEBUG_OUT
                printf("SOLVED\n");
            #endif

            #ifdef JM_VERBOSE_OUT
                printf("\nIterations: %lu\n", interactions);
                printf("RowTest: %lu => [%lf] =? %lf\n",
                M->J_ROW_TEST, M->vector[M->J_ROW_TEST]-E,
                M->vector[M->J_ROW_TEST]);
            #endif
            return Xk_1; // Solved
        }

        if(interactions > M->J_ITE_MAX) break;
    }
    #ifdef JM_DEBUG_OUT
        printf("MAX_INTER_HALT\n");
    #endif

    #ifdef JM_VERBOSE_OUT
        printf("\nIterations: %lu - MAX INTERACTIONS HALT\n"
        , interactions);
        printf("RowTest: %lu => [%lf] =? %lf\n",
        M->J_ROW_TEST, M->vector[M->J_ROW_TEST]-E,
        M->vector[M->J_ROW_TEST]);
    #endif
    
    return MAX_INTER_HALT;
}

inline double jmFindXi(struct extendedMatrix *M,
                  unsigned long line,
                  double *X){
    double Xi, Ad, Aij;
    unsigned long j;

    // Xi = b
    Xi = M->vector[line];    

    for(j=0; j<M->J_ORDER; j++){
        Aij = M->matrix[line][j];
        if(j==line){
            // Element from the diagonal
            Ad = Aij;
            continue;
        }else{
            // Not from the diagonal
            // Xi = b-SUM(Aij*Xk)
            Xi -= Aij*X[j];
        }
    }
    Xi/=Ad;
            /*  Xk+1 = b-SUM(Aij*Xk)
                       -------------
                           Aii        */
    return Xi;
}

inline double numericValidation(struct extendedMatrix *M, double *Xn){
    int j, N, I;
    double R=0, E;

    N = M->J_ORDER;
    I = M->J_ROW_TEST;

    for(j=0; j<N; j++)
        R += (M->matrix[I][j])*(Xn[j]);

    #ifdef JM_DEBUG_NVAL
        printf("R=%lf B=%lf ", R, M->vector[I]);
    #endif
    // We assume the positive value
    E = relativeError(R, M->vector[I]);    
    
    #ifdef JM_DEBUG_NVAL
        printf("E=%lf \n", E);
    #endif

    // If the functions return an negative value, its because
    // the error is not acceptable;
    if(E > M->J_ERROR) return -E;

    return E;
}

inline double relativeError(double x, double y){
    return fabs(((x-y)/x));
}
